diff --git a/tools/kwboot.c b/tools/kwboot.c
index 77bf5cb..ba2fd10 100644
--- a/tools/kwboot.c
+++ b/tools/kwboot.c
@@ -70,6 +70,187 @@
 #define KWBOOT_BLK_RSP_TIMEO 1000 /* ms */
 #define KWBOOT_HDR_RSP_TIMEO 10000 /* ms */
 
+/* ARM code making baudrate changing function return to original exec address */
+static unsigned char kwboot_pre_baud_code[] = {
+				/* exec_addr:                                 */
+	0x00, 0x00, 0x00, 0x00, /* .word 0                                    */
+	0x0c, 0xe0, 0x1f, 0xe5, /* ldr lr, exec_addr                          */
+};
+
+/* ARM code for binary header injection to change baudrate */
+static unsigned char kwboot_baud_code[] = {
+				/* ; #define UART_BASE 0xd0012000             */
+				/* ; #define THR       0x00                   */
+				/* ; #define DLL       0x00                   */
+				/* ; #define DLH       0x04                   */
+				/* ; #define LCR       0x0c                   */
+				/* ; #define   DLAB    0x80                   */
+				/* ; #define LSR       0x14                   */
+				/* ; #define   THRE    0x20                   */
+				/* ; #define   TEMT    0x40                   */
+				/* ; #define DIV_ROUND(a, b) ((a + b/2) / b)  */
+				/* ;                                          */
+				/* ; u32 set_baudrate(u32 old_b, u32 new_b) { */
+				/* ;   const u8 *str = "$baudratechange";     */
+				/* ;   u8 c;                                  */
+				/* ;   do {                                   */
+				/* ;       c = *str++;                        */
+				/* ;       writel(UART_BASE + THR, c);        */
+				/* ;   } while (c);                           */
+				/* ;   while                                  */
+				/* ;      (!(readl(UART_BASE + LSR) & TEMT)); */
+				/* ;   u32 lcr = readl(UART_BASE + LCR);      */
+				/* ;   writel(UART_BASE + LCR, lcr | DLAB);   */
+				/* ;   u8 old_dll = readl(UART_BASE + DLL);   */
+				/* ;   u8 old_dlh = readl(UART_BASE + DLH);   */
+				/* ;   u16 old_dl = old_dll | (old_dlh << 8); */
+				/* ;   u32 clk = old_b * old_dl;              */
+				/* ;   u16 new_dl = DIV_ROUND(clk, new_b);    */
+				/* ;   u8 new_dll = new_dl & 0xff;            */
+				/* ;   u8 new_dlh = (new_dl >> 8) & 0xff;     */
+				/* ;   writel(UART_BASE + DLL, new_dll);      */
+				/* ;   writel(UART_BASE + DLH, new_dlh);      */
+				/* ;   writel(UART_BASE + LCR, lcr & ~DLAB);  */
+				/* ;   msleep(1);                             */
+				/* ;   return 0;                              */
+				/* ; }                                        */
+
+	0xfe, 0x5f, 0x2d, 0xe9, /* push  { r1 - r12, lr }                     */
+
+				/*  ; r0 = UART_BASE                          */
+	0x02, 0x0a, 0xa0, 0xe3, /* mov   r0, #0x2000                          */
+	0x01, 0x00, 0x4d, 0xe3, /* movt  r0, #0xd001                          */
+
+				/*  ; r2 = address of preamble string         */
+	0xd0, 0x20, 0x8f, 0xe2, /* adr   r2, preamble                         */
+
+				/*  ; Send preamble string over UART          */
+				/* .Lloop_preamble:                           */
+				/*                                            */
+				/*  ; Wait until Transmitter Holding is Empty */
+				/* .Lloop_thre:                               */
+				/*  ; r1 = UART_BASE[LSR] & THRE              */
+	0x14, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x14]                      */
+	0x20, 0x00, 0x11, 0xe3, /* tst   r1, #0x20                            */
+	0xfc, 0xff, 0xff, 0x0a, /* beq   .Lloop_thre                          */
+
+				/*  ; Put character into Transmitter FIFO     */
+				/*  ; r1 = *r2++                              */
+	0x01, 0x10, 0xd2, 0xe4, /* ldrb  r1, [r2], #1                         */
+				/*  ; UART_BASE[THR] = r1                     */
+	0x00, 0x10, 0x80, 0xe5, /* str   r1, [r0, #0x0]                       */
+
+				/*  ; Loop until end of preamble string       */
+	0x00, 0x00, 0x51, 0xe3, /* cmp   r1, #0                               */
+	0xf8, 0xff, 0xff, 0x1a, /* bne   .Lloop_preamble                      */
+
+				/*  ; Wait until Transmitter FIFO is Empty    */
+				/* .Lloop_txempty:                            */
+				/*  ; r1 = UART_BASE[LSR] & TEMT              */
+	0x14, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x14]                      */
+	0x40, 0x00, 0x11, 0xe3, /* tst   r1, #0x40                            */
+	0xfc, 0xff, 0xff, 0x0a, /* beq   .Lloop_txempty                       */
+
+				/*  ; Set Divisor Latch Access Bit            */
+				/*  ; UART_BASE[LCR] |= DLAB                  */
+	0x0c, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x0c]                      */
+	0x80, 0x10, 0x81, 0xe3, /* orr   r1, r1, #0x80                        */
+	0x0c, 0x10, 0x80, 0xe5, /* str   r1, [r0, #0x0c]                      */
+
+				/*  ; Read current Divisor Latch              */
+				/*  ; r1 = UART_BASE[DLH]<<8 | UART_BASE[DLL] */
+	0x00, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x00]                      */
+	0xff, 0x10, 0x01, 0xe2, /* and   r1, r1, #0xff                        */
+	0x01, 0x20, 0xa0, 0xe1, /* mov   r2, r1                               */
+	0x04, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x04]                      */
+	0xff, 0x10, 0x01, 0xe2, /* and   r1, r1, #0xff                        */
+	0x41, 0x14, 0xa0, 0xe1, /* asr   r1, r1, #8                           */
+	0x02, 0x10, 0x81, 0xe1, /* orr   r1, r1, r2                           */
+
+				/*  ; Read old baudrate value                 */
+				/*  ; r2 = old_baudrate                       */
+	0x8c, 0x20, 0x9f, 0xe5, /* ldr   r2, old_baudrate                     */
+
+				/*  ; Calculate base clock                    */
+				/*  ; r1 = r2 * r1                            */
+	0x92, 0x01, 0x01, 0xe0, /* mul   r1, r2, r1                           */
+
+				/*  ; Read new baudrate value                 */
+				/*  ; r2 = baudrate                           */
+	0x88, 0x20, 0x9f, 0xe5, /* ldr   r2, baudrate                         */
+
+				/*  ; Calculate new Divisor Latch             */
+				/*  ; r1 = DIV_ROUND(r1, r2) =                */
+				/*  ;    = (r1 + r2/2) / r2                   */
+	0xa2, 0x10, 0x81, 0xe0, /* add   r1, r1, r2, lsr #1                   */
+	0x02, 0x40, 0xa0, 0xe1, /* mov   r4, r2                               */
+	0xa1, 0x00, 0x54, 0xe1, /* cmp   r4, r1, lsr #1                       */
+				/* .Lloop_div1:                               */
+	0x84, 0x40, 0xa0, 0x91, /* movls r4, r4, lsl #1                       */
+	0xa1, 0x00, 0x54, 0xe1, /* cmp   r4, r1, lsr #1                       */
+	0xfc, 0xff, 0xff, 0x9a, /* bls   .Lloop_div1                          */
+	0x00, 0x30, 0xa0, 0xe3, /* mov   r3, #0                               */
+				/* .Lloop_div2:                               */
+	0x04, 0x00, 0x51, 0xe1, /* cmp   r1, r4                               */
+	0x04, 0x10, 0x41, 0x20, /* subhs r1, r1, r4                           */
+	0x03, 0x30, 0xa3, 0xe0, /* adc   r3, r3, r3                           */
+	0xa4, 0x40, 0xa0, 0xe1, /* mov   r4, r4, lsr #1                       */
+	0x02, 0x00, 0x54, 0xe1, /* cmp   r4, r2                               */
+	0xf9, 0xff, 0xff, 0x2a, /* bhs   .Lloop_div2                          */
+	0x03, 0x10, 0xa0, 0xe1, /* mov   r1, r3                               */
+
+				/*  ; Set new Divisor Latch Low               */
+				/*  ; UART_BASE[DLL] = r1 & 0xff              */
+	0x01, 0x20, 0xa0, 0xe1, /* mov   r2, r1                               */
+	0xff, 0x20, 0x02, 0xe2, /* and   r2, r2, #0xff                        */
+	0x00, 0x20, 0x80, 0xe5, /* str   r2, [r0, #0x00]                      */
+
+				/*  ; Set new Divisor Latch High              */
+				/*  ; UART_BASE[DLH] = r1>>8 & 0xff           */
+	0x41, 0x24, 0xa0, 0xe1, /* asr   r2, r1, #8                           */
+	0xff, 0x20, 0x02, 0xe2, /* and   r2, r2, #0xff                        */
+	0x04, 0x20, 0x80, 0xe5, /* str   r2, [r0, #0x04]                      */
+
+				/*  ; Clear Divisor Latch Access Bit          */
+				/*  ; UART_BASE[LCR] &= ~DLAB                 */
+	0x0c, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x0c]                      */
+	0x80, 0x10, 0xc1, 0xe3, /* bic   r1, r1, #0x80                        */
+	0x0c, 0x10, 0x80, 0xe5, /* str   r1, [r0, #0x0c]                      */
+
+				/*  ; Sleep 1ms ~~ 600000 cycles at 1200 MHz  */
+				/*  ; r1 = 600000                             */
+	0x9f, 0x1d, 0xa0, 0xe3, /* mov   r1, #0x27c0                          */
+	0x09, 0x10, 0x40, 0xe3, /* movt  r1, #0x0009                          */
+				/* .Lloop_sleep:                              */
+	0x01, 0x10, 0x41, 0xe2, /* sub   r1, r1, #1                           */
+	0x00, 0x00, 0x51, 0xe3, /* cmp   r1, #0                               */
+	0xfc, 0xff, 0xff, 0x1a, /* bne   .Lloop_sleep                         */
+
+				/*  ; Return 0 - no error                     */
+	0x00, 0x00, 0xa0, 0xe3, /* mov   r0, #0                               */
+	0xfe, 0x9f, 0xbd, 0xe8, /* pop   { r1 - r12, pc }                     */
+
+				/*  ; Preamble string                         */
+				/* preamble:                                  */
+	0x24, 0x62, 0x61, 0x75, /* .asciz "$baudratechange"                   */
+	0x64, 0x72, 0x61, 0x74,
+	0x65, 0x63, 0x68, 0x61,
+	0x6e, 0x67, 0x65, 0x00,
+
+				/*  ; Placeholder for old baudrate value      */
+				/* old_baudrate:                              */
+	0x00, 0x00, 0x00, 0x00, /* .word 0                                    */
+
+				/*  ; Placeholder for new baudrate value      */
+				/* new_baudrate:                              */
+	0x00, 0x00, 0x00, 0x00, /* .word 0                                    */
+};
+
+#define KWBOOT_BAUDRATE_BIN_HEADER_SZ (sizeof(kwboot_baud_code) + \
+				       sizeof(struct opt_hdr_v1) + 8)
+
+static const char kwb_baud_magic[16] = "$baudratechange";
+
 static int kwboot_verbose;
 
 static int msg_req_delay = KWBOOT_MSG_REQ_DELAY;
@@ -233,26 +414,184 @@
 }
 
 static speed_t
-kwboot_tty_speed(int baudrate)
+kwboot_tty_baudrate_to_speed(int baudrate)
 {
 	switch (baudrate) {
+#ifdef B4000000
+	case 4000000:
+		return B4000000;
+#endif
+#ifdef B3500000
+	case 3500000:
+		return B3500000;
+#endif
+#ifdef B3000000
+	case 3000000:
+		return B3000000;
+#endif
+#ifdef B2500000
+	case 2500000:
+		return B2500000;
+#endif
+#ifdef B2000000
+	case 2000000:
+		return B2000000;
+#endif
+#ifdef B1500000
+	case 1500000:
+		return B1500000;
+#endif
+#ifdef B1152000
+	case 1152000:
+		return B1152000;
+#endif
+#ifdef B1000000
+	case 1000000:
+		return B1000000;
+#endif
+#ifdef B921600
+	case 921600:
+		return B921600;
+#endif
+#ifdef B614400
+	case 614400:
+		return B614400;
+#endif
+#ifdef B576000
+	case 576000:
+		return B576000;
+#endif
+#ifdef B500000
+	case 500000:
+		return B500000;
+#endif
+#ifdef B460800
+	case 460800:
+		return B460800;
+#endif
+#ifdef B307200
+	case 307200:
+		return B307200;
+#endif
+#ifdef B230400
+	case 230400:
+		return B230400;
+#endif
+#ifdef B153600
+	case 153600:
+		return B153600;
+#endif
+#ifdef B115200
 	case 115200:
 		return B115200;
+#endif
+#ifdef B76800
+	case 76800:
+		return B76800;
+#endif
+#ifdef B57600
 	case 57600:
 		return B57600;
+#endif
+#ifdef B38400
 	case 38400:
 		return B38400;
+#endif
+#ifdef B19200
 	case 19200:
 		return B19200;
+#endif
+#ifdef B9600
 	case 9600:
 		return B9600;
+#endif
+#ifdef B4800
+	case 4800:
+		return B4800;
+#endif
+#ifdef B2400
+	case 2400:
+		return B2400;
+#endif
+#ifdef B1800
+	case 1800:
+		return B1800;
+#endif
+#ifdef B1200
+	case 1200:
+		return B1200;
+#endif
+#ifdef B600
+	case 600:
+		return B600;
+#endif
+#ifdef B300
+	case 300:
+		return B300;
+#endif
+#ifdef B200
+	case 200:
+		return B200;
+#endif
+#ifdef B150
+	case 150:
+		return B150;
+#endif
+#ifdef B134
+	case 134:
+		return B134;
+#endif
+#ifdef B110
+	case 110:
+		return B110;
+#endif
+#ifdef B75
+	case 75:
+		return B75;
+#endif
+#ifdef B50
+	case 50:
+		return B50;
+#endif
+	default:
+		return B0;
 	}
-
-	return -1;
 }
 
 static int
-kwboot_open_tty(const char *path, speed_t speed)
+kwboot_tty_change_baudrate(int fd, int baudrate)
+{
+	struct termios tio;
+	speed_t speed;
+	int rc;
+
+	rc = tcgetattr(fd, &tio);
+	if (rc)
+		return rc;
+
+	speed = kwboot_tty_baudrate_to_speed(baudrate);
+	if (speed == B0) {
+		errno = EINVAL;
+		return -1;
+	}
+
+	rc = cfsetospeed(&tio, speed);
+	if (rc)
+		return rc;
+
+	rc = cfsetispeed(&tio, speed);
+	if (rc)
+		return rc;
+
+	rc = tcsetattr(fd, TCSANOW, &tio);
+	if (rc)
+		return rc;
+
+	return 0;
+}
+
+static int
+kwboot_open_tty(const char *path, int baudrate)
 {
 	int rc, fd;
 	struct termios tio;
@@ -271,13 +610,14 @@
 	tio.c_cc[VMIN] = 1;
 	tio.c_cc[VTIME] = 10;
 
-	cfsetospeed(&tio, speed);
-	cfsetispeed(&tio, speed);
-
 	rc = tcsetattr(fd, TCSANOW, &tio);
 	if (rc)
 		goto out;
 
+	rc = kwboot_tty_change_baudrate(fd, baudrate);
+	if (rc)
+		goto out;
+
 	rc = fd;
 out:
 	if (rc < 0) {
@@ -426,7 +766,34 @@
 }
 
 static int
-kwboot_xm_recv_reply(int fd, char *c, int allow_non_xm, int *non_xm_print)
+kwboot_baud_magic_handle(int fd, char c, int baudrate)
+{
+	static size_t rcv_len;
+
+	if (rcv_len < sizeof(kwb_baud_magic)) {
+		/* try to recognize whole magic word */
+		if (c == kwb_baud_magic[rcv_len]) {
+			rcv_len++;
+		} else {
+			printf("%.*s%c", (int)rcv_len, kwb_baud_magic, c);
+			fflush(stdout);
+			rcv_len = 0;
+		}
+	}
+
+	if (rcv_len == sizeof(kwb_baud_magic)) {
+		/* magic word received */
+		kwboot_printv("\nChanging baudrate to %d Bd\n", baudrate);
+
+		return kwboot_tty_change_baudrate(fd, baudrate) ? : 1;
+	} else {
+		return 0;
+	}
+}
+
+static int
+kwboot_xm_recv_reply(int fd, char *c, int allow_non_xm, int *non_xm_print,
+		     int baudrate, int *baud_changed)
 {
 	int timeout = allow_non_xm ? KWBOOT_HDR_RSP_TIMEO : blk_rsp_timeo;
 	uint64_t recv_until = _now() + timeout;
@@ -434,6 +801,8 @@
 
 	if (non_xm_print)
 		*non_xm_print = 0;
+	if (baud_changed)
+		*baud_changed = 0;
 
 	while (1) {
 		rc = kwboot_tty_recv(fd, c, 1, timeout);
@@ -451,15 +820,30 @@
 			break;
 
 		/*
-		 * If printing non-xmodem text output is allowed and such a byte
-		 * was received, print it and increase receiving time.
+		 * If receiving/printing non-xmodem text output is allowed and
+		 * such a byte was received, we want to increase receiving time
+		 * and either:
+		 * - print the byte, if it is not part of baudrate change magic
+		 *   sequence while baudrate change was requested (-B option)
+		 * - change baudrate
 		 * Otherwise decrease timeout by time elapsed.
 		 */
 		if (allow_non_xm) {
 			recv_until = _now() + timeout;
-			putchar(*c);
-			fflush(stdout);
-			*non_xm_print = 1;
+
+			if (baudrate && !*baud_changed) {
+				rc = kwboot_baud_magic_handle(fd, *c, baudrate);
+				if (rc == 1)
+					*baud_changed = 1;
+				else if (!rc)
+					*non_xm_print = 1;
+				else
+					return rc;
+			} else if (!baudrate || !*baud_changed) {
+				putchar(*c);
+				fflush(stdout);
+				*non_xm_print = 1;
+			}
 		} else {
 			timeout = recv_until - _now();
 			if (timeout < 0) {
@@ -474,10 +858,10 @@
 
 static int
 kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm,
-		    int *done_print)
+		    int *done_print, int baudrate)
 {
-	int non_xm_print;
-	int rc, retries;
+	int non_xm_print, baud_changed;
+	int rc, err, retries;
 	char c;
 
 	*done_print = 0;
@@ -494,9 +878,10 @@
 			*done_print = 1;
 		}
 
-		rc = kwboot_xm_recv_reply(fd, &c, allow_non_xm, &non_xm_print);
+		rc = kwboot_xm_recv_reply(fd, &c, allow_non_xm, &non_xm_print,
+					  baudrate, &baud_changed);
 		if (rc)
-			return rc;
+			goto can;
 
 		if (!allow_non_xm && c != ACK)
 			kwboot_progress(-1, '+');
@@ -505,7 +890,20 @@
 	if (non_xm_print)
 		kwboot_printv("\n");
 
+	if (allow_non_xm && baudrate && !baud_changed) {
+		fprintf(stderr, "Baudrate was not changed\n");
+		rc = -1;
+		errno = EPROTO;
+		goto can;
+	}
+
 	return _xm_reply_to_error(c);
+can:
+	err = errno;
+	kwboot_tty_send_char(fd, CAN);
+	kwboot_printv("\n");
+	errno = err;
+	return rc;
 }
 
 static int
@@ -522,7 +920,7 @@
 		if (rc)
 			return rc;
 
-		rc = kwboot_xm_recv_reply(fd, &c, 0, NULL);
+		rc = kwboot_xm_recv_reply(fd, &c, 0, NULL, 0, NULL);
 		if (rc)
 			return rc;
 	} while (c == NAK && retries-- > 0);
@@ -532,7 +930,7 @@
 
 static int
 kwboot_xmodem_one(int tty, int *pnum, int header, const uint8_t *data,
-		  size_t size)
+		  size_t size, int baudrate)
 {
 	int done_print = 0;
 	size_t sent, left;
@@ -555,7 +953,7 @@
 		last_block = (left <= blksz);
 
 		rc = kwboot_xm_sendblock(tty, &block, header && last_block,
-					 &done_print);
+					 &done_print, baudrate);
 		if (rc)
 			goto out;
 
@@ -576,7 +974,7 @@
 }
 
 static int
-kwboot_xmodem(int tty, const void *_img, size_t size)
+kwboot_xmodem(int tty, const void *_img, size_t size, int baudrate)
 {
 	const uint8_t *img = _img;
 	int rc, pnum;
@@ -590,18 +988,41 @@
 
 	pnum = 1;
 
-	rc = kwboot_xmodem_one(tty, &pnum, 1, img, hdrsz);
+	rc = kwboot_xmodem_one(tty, &pnum, 1, img, hdrsz, baudrate);
 	if (rc)
 		return rc;
 
 	img += hdrsz;
 	size -= hdrsz;
 
-	rc = kwboot_xmodem_one(tty, &pnum, 0, img, size);
+	rc = kwboot_xmodem_one(tty, &pnum, 0, img, size, 0);
 	if (rc)
 		return rc;
 
-	return kwboot_xm_finish(tty);
+	rc = kwboot_xm_finish(tty);
+	if (rc)
+		return rc;
+
+	if (baudrate) {
+		char buf[sizeof(kwb_baud_magic)];
+
+		/* Wait 1s for baudrate change magic */
+		rc = kwboot_tty_recv(tty, buf, sizeof(buf), 1000);
+		if (rc)
+			return rc;
+
+		if (memcmp(buf, kwb_baud_magic, sizeof(buf))) {
+			errno = EPROTO;
+			return -1;
+		}
+
+		kwboot_printv("\nChanging baudrate back to 115200 Bd\n\n");
+		rc = kwboot_tty_change_baudrate(tty, 115200);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
 }
 
 static int
@@ -782,6 +1203,37 @@
 	return 0;
 }
 
+static void *
+kwboot_img_grow_data_left(void *img, size_t *size, size_t grow)
+{
+	uint32_t hdrsz, datasz, srcaddr;
+	struct main_hdr_v1 *hdr = img;
+	uint8_t *data;
+
+	srcaddr = le32_to_cpu(hdr->srcaddr);
+
+	hdrsz = kwbheader_size(hdr);
+	data = (uint8_t *)img + srcaddr;
+	datasz = *size - srcaddr;
+
+	/* only move data if there is not enough space */
+	if (hdrsz + grow > srcaddr) {
+		size_t need = hdrsz + grow - srcaddr;
+
+		/* move data by enough bytes */
+		memmove(data + need, data, datasz);
+		*size += need;
+		srcaddr += need;
+	}
+
+	srcaddr -= grow;
+	hdr->srcaddr = cpu_to_le32(srcaddr);
+	hdr->destaddr = cpu_to_le32(le32_to_cpu(hdr->destaddr) - grow);
+	hdr->blocksize = cpu_to_le32(le32_to_cpu(hdr->blocksize) + grow);
+
+	return (uint8_t *)img + srcaddr;
+}
+
 static void
 kwboot_img_grow_hdr(void *img, size_t *size, size_t grow)
 {
@@ -813,8 +1265,71 @@
 	}
 }
 
+static void *
+kwboot_add_bin_ohdr_v1(void *img, size_t *size, uint32_t binsz)
+{
+	struct main_hdr_v1 *hdr = img;
+	struct opt_hdr_v1 *ohdr;
+	uint32_t ohdrsz;
+
+	ohdrsz = binsz + 8 + sizeof(*ohdr);
+	kwboot_img_grow_hdr(img, size, ohdrsz);
+
+	if (hdr->ext & 0x1) {
+		for_each_opt_hdr_v1 (ohdr, img)
+			if (opt_hdr_v1_next(ohdr) == NULL)
+				break;
+
+		*opt_hdr_v1_ext(ohdr) |= 1;
+		ohdr = opt_hdr_v1_next(ohdr);
+	} else {
+		hdr->ext |= 1;
+		ohdr = (void *)(hdr + 1);
+	}
+
+	ohdr->headertype = OPT_HDR_V1_BINARY_TYPE;
+	ohdr->headersz_msb = ohdrsz >> 16;
+	ohdr->headersz_lsb = cpu_to_le16(ohdrsz & 0xffff);
+
+	memset(&ohdr->data[0], 0, ohdrsz - sizeof(*ohdr));
+
+	return &ohdr->data[4];
+}
+
+static void
+_copy_baudrate_change_code(struct main_hdr_v1 *hdr, void *dst, int pre,
+			   int old_baud, int new_baud)
+{
+	size_t codesz = sizeof(kwboot_baud_code);
+	uint8_t *code = dst;
+
+	if (pre) {
+		size_t presz = sizeof(kwboot_pre_baud_code);
+
+		/*
+		 * We need to prepend code that loads lr register with original
+		 * value of hdr->execaddr. We do this by putting the original
+		 * exec address before the code that loads it relatively from
+		 * it's beginning.
+		 * Afterwards we change the exec address to this code (which is
+		 * at offset 4, because the first 4 bytes contain the original
+		 * exec address).
+		 */
+		memcpy(code, kwboot_pre_baud_code, presz);
+		*(uint32_t *)code = hdr->execaddr;
+
+		hdr->execaddr = cpu_to_le32(le32_to_cpu(hdr->destaddr) + 4);
+
+		code += presz;
+	}
+
+	memcpy(code, kwboot_baud_code, codesz - 8);
+	*(uint32_t *)(code + codesz - 8) = cpu_to_le32(old_baud);
+	*(uint32_t *)(code + codesz - 4) = cpu_to_le32(new_baud);
+}
+
 static int
-kwboot_img_patch_hdr(void *img, size_t *size)
+kwboot_img_patch(void *img, size_t *size, int baudrate)
 {
 	int rc;
 	struct main_hdr_v1 *hdr;
@@ -908,6 +1423,51 @@
 		hdr->blockid = IBR_HDR_UART_ID;
 	}
 
+	if (baudrate) {
+		uint32_t codesz = sizeof(kwboot_baud_code);
+		void *code;
+
+		if (image_ver == 0) {
+			fprintf(stderr,
+				"Cannot inject code for changing baudrate into v0 image header\n");
+			errno = EINVAL;
+			goto out;
+		}
+
+		if (is_secure) {
+			fprintf(stderr,
+				"Cannot inject code for changing baudrate into image with secure header\n");
+			errno = EINVAL;
+			goto out;
+		}
+
+		/*
+		 * First inject code that changes the baudrate from the default
+		 * value of 115200 Bd to requested value. This code is inserted
+		 * as a new opt hdr, so it is executed by BootROM after the
+		 * header part is received.
+		 */
+		kwboot_printv("Injecting binary header code for changing baudrate to %d Bd\n",
+			      baudrate);
+
+		code = kwboot_add_bin_ohdr_v1(img, size, codesz);
+		_copy_baudrate_change_code(hdr, code, 0, 115200, baudrate);
+
+		/*
+		 * Now inject code that changes the baudrate back to 115200 Bd.
+		 * This code is prepended to the data part of the image, so it
+		 * is executed before U-Boot proper.
+		 */
+		kwboot_printv("Injecting code for changing baudrate back\n");
+
+		codesz += sizeof(kwboot_pre_baud_code);
+		code = kwboot_img_grow_data_left(img, size, codesz);
+		_copy_baudrate_change_code(hdr, code, 1, baudrate, 115200);
+
+		/* recompute header size */
+		hdrsz = kwbheader_size(hdr);
+	}
+
 	if (hdrsz % KWBOOT_XM_BLKSZ) {
 		size_t offset = (KWBOOT_XM_BLKSZ - hdrsz % KWBOOT_XM_BLKSZ) %
 				KWBOOT_XM_BLKSZ;
@@ -964,7 +1524,8 @@
 	void *debugmsg;
 	void *img;
 	size_t size;
-	speed_t speed;
+	size_t after_img_rsv;
+	int baudrate;
 
 	rv = 1;
 	tty = -1;
@@ -974,7 +1535,8 @@
 	img = NULL;
 	term = 0;
 	size = 0;
-	speed = B115200;
+	after_img_rsv = KWBOOT_XM_BLKSZ;
+	baudrate = 115200;
 
 	kwboot_verbose = isatty(STDOUT_FILENO);
 
@@ -1024,9 +1586,7 @@
 			break;
 
 		case 'B':
-			speed = kwboot_tty_speed(atoi(optarg));
-			if (speed == -1)
-				goto usage;
+			baudrate = atoi(optarg);
 			break;
 
 		case 'h':
@@ -1044,20 +1604,29 @@
 
 	ttypath = argv[optind++];
 
-	tty = kwboot_open_tty(ttypath, speed);
+	tty = kwboot_open_tty(ttypath, imgpath ? 115200 : baudrate);
 	if (tty < 0) {
 		perror(ttypath);
 		goto out;
 	}
 
+	if (baudrate == 115200)
+		/* do not change baudrate during Xmodem to the same value */
+		baudrate = 0;
+	else
+		/* ensure we have enough space for baudrate change code */
+		after_img_rsv += KWBOOT_BAUDRATE_BIN_HEADER_SZ +
+				 sizeof(kwboot_pre_baud_code) +
+				 sizeof(kwboot_baud_code);
+
 	if (imgpath) {
-		img = kwboot_read_image(imgpath, &size, KWBOOT_XM_BLKSZ);
+		img = kwboot_read_image(imgpath, &size, after_img_rsv);
 		if (!img) {
 			perror(imgpath);
 			goto out;
 		}
 
-		rc = kwboot_img_patch_hdr(img, &size);
+		rc = kwboot_img_patch(img, &size, baudrate);
 		if (rc) {
 			fprintf(stderr, "%s: Invalid image.\n", imgpath);
 			goto out;
@@ -1079,7 +1648,7 @@
 	}
 
 	if (img) {
-		rc = kwboot_xmodem(tty, img, size);
+		rc = kwboot_xmodem(tty, img, size, baudrate);
 		if (rc) {
 			perror("xmodem");
 			goto out;